/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#define MAP_FILE_NAME "../../res/autmap-small.txt"
#define OCCU_RESOLUTION 0.1

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

TOccupancyGrid *O; ///< Occupancy Grid 
float *dist=NULL;	 ///< Distance transform	
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
		rgb[3*Nx*j+3*i]=r;
		rgb[3*Nx*j+3*i+1]=g;
		rgb[3*Nx*j+3*i+2]=b;
}
void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
		float *data = gr->getDataPtr();
		int Nx,Ny;
		gr->getDim(Nx,Ny);
		float v;
	
		for(int j=0;j<Ny;j++){
				for(int i=0;i<Nx;i++){
						v = data[Nx*j+i];
				//if(v>0.29) v = 0.9;
						if(v<0.6) v = 0.4;
						setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
				}
		}	
}
void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}
}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											RGB[3*Nx*y+3*x] = r;
											RGB[3*Nx*y+3*x+1] = g;
											RGB[3*Nx*y+3*x+2] = b;
											i++;
											rr+=resolution;
									}
}
void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
/**
* returns distance to nearest obstacle from position (x,y) (in the map coordinate system)
* @param *dt distance transform
* @param *g  pointer to occupancy grid
* @param x   x-coordinate
* @param y   y-coordinate
* @return distance to nearest obstacle
*/
float getDist(float *dt,TOccupancyGrid *g,float x, float y){
	int Nx,Ny;
	Grid::TPixel pix= g->toPixel(Grid::TCoord2D(x,y));
	g->getDim(Nx,Ny);
	
	if(pix.x >=0 && pix.x <Nx &&pix.y>=0&&pix.y<Ny){
		return dt[Nx*pix.y + pix.x];
	}
	return -1;
}


int main(int argc, char *argv[]){
		int Nx,Ny;
		
		/**
		* Initialize the occupancy grid map
		*/
		O = new TOccupancyGrid(1,1,OCCU_RESOLUTION,Grid::TCoord2D(0,0));
		if(!O->loadLineMap(MAP_FILE_NAME, 1.0,1.0)){
				fprintf(stderr,"**** Unable to load the map\n");
				exit(1);
		}
		O->getDim(Nx,Ny); ///<Number of pixels in the occupancy grid
		dist = (float *) malloc(3*Nx*Ny); /// memory for distance transform
		
		
		/**
		* These are for displaying only
		*/
		unsigned char *rgb = (unsigned char *) malloc(3*Nx*Ny); ///< the image
		addImageWindow(Nx, Ny); ///< initialize imageGUI
		glutImageGUI_Start();   ///< Start the imagegui
		setRGBmap(rgb,O, 220,220, 220); ///< set the map to image
		displayImage(0, rgb,Ny,Nx); ///< and display
		fprintf(stderr,"Le Map\n");
		usleep(1000*1000);
		saveJPEG("map.jpg",rgb,Nx,Ny);
		
		float maxdist = O->distanceTransform(dist, 0.9); ///< Compute distance transform			
		
		for(int i = 0;i<Nx*Ny;i++){
			float v = 255 * dist[i]/maxdist;
			rgb[3*i] = v;rgb[3*i+1] = v;rgb[3*i+2] = v;
		}
		
		fprintf(stderr,"Le Dist-Transform\n");
		displayImage(0, rgb,Ny,Nx); ///< and display
		usleep(1000*1000);
		saveJPEG("dist.jpg",rgb,Nx,Ny);
		
		int cnt=0;
		float v=0;		
		
		fprintf(stderr,"Getting a couple of million distances\n");
 		long int ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss1,&us1);
		
		for(int i=0;i<20*1000;i++){
			for(int j=0;j<1000;j++){
				v += 0.001*getDist(dist, O, i/1000,j/100);
				if(v>200000) v = 0;
			}
		}
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"Done time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		
		fprintf(stderr,"\nv = %f\n",v);
		
		return 0;
}		
